#include <termios.h>
#include <stdlib.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>

int serial_fd = -1;

void moveToPosition(int x, int y, int z, int roll, int pitch);
void testMove();
void setUpConnection();

main(){
	testMove();
}

void testMove()
{
	setUpConnection();
	printf("Please click a button on the controller to proceed\n");
	getchar();
	moveToPosition(4000, 2000, 2000, 0, 0);
	sleep(2);
	moveToPosition(3500, 1200, -2000, -900, 0);
}

void setUpConnection()
{
	struct termios scorbot_config; //sets up termios configuration structure for the scorbot serial port -go
	
	const char *device = "/dev/ttyS0"; //sets where the serial port is plugged in -go
	serial_fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY); //opens serial port in device slot -go
	if(serial_fd == -1) {
		fprintf(stderr, "failed to open serial port\n");
		exit(-1);
	}
	
	tcgetattr(serial_fd, &scorbot_config);
	
	cfsetispeed(&scorbot_config, B9600); //set baud input to 9600
	cfsetospeed(&scorbot_config, B9600); //set baud output to 9600
	
	scorbot_config.c_iflag = 0;
	scorbot_config.c_iflag &= (IXON | IXOFF);
	
	scorbot_config.c_oflag = 0;

	scorbot_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	
	scorbot_config.c_cflag = 0;
	scorbot_config.c_cflag &= (CLOCAL | CREAD | CS8); //enable receiver and set to local mode
		
	
	scorbot_config.c_cc[VMIN]  = 1;
	scorbot_config.c_cc[VTIME] = 0;
	
	tcsetattr(serial_fd, TCSAFLUSH, &scorbot_config);
}



void
moveToPosition(int x, int y, int z, int roll, int pitch)
{	
	fprintf(stdout, "Sending coordinates to robot.\n"); //-debug -go
	char commandString[200];
	sprintf(commandString, "DEFP COORD\rTEACH COORD\r%d\r%d\r%d\r%d\r%d\rMOVE COORD\r", x,y,z,roll,pitch);
	write(serial_fd, commandString, strlen(commandString));
	//write(serial_fd, input, strlen(input));
	//fprintf(serial_fd, "DEFP COORD\rTEACH COORD\r%d\r%d\r%d\r%d\r%d\rMOVE COORD\r", x,y,z,roll,pitch);
}
